#include "speed_close_loop.h"
#include "vofa.h"

PIDController pid_q = {
    .kp = 1.0,
    .ki = 12.0,
    .kd = 0.0,
    /* 导数低通滤波器时间常数 */
    .tau = 0.0055,
    /* Output limits */
    .limMin = -1.0,
    .limMax = +1.0,
    /* Integrator limits */
    .limMinInt = -0.5,
    .limMaxInt = +0.5,
    /* Sample time (in seconds) */
    .T = 0.005, // 采样时间 5ms
    /* Controller "memory" */
    .integrator = 0.0,
    .prevError = 0.0,
    .differentiator = 0.0,
    .prevMeasurement = 0.0,
    /* Controller output */
    .out = 0.0,
}; // PID 控制器实例

PIDController pid_d = {
    .kp = 1.5,
    .ki = 12.0,
    .kd = 0.0,
    /* 导数低通滤波器时间常数 */
    .tau = 0.0055,
    /* Output limits */
    .limMin = -1.0,
    .limMax = +1.0,
    /* Integrator limits */
    .limMinInt = -0.5,
    .limMaxInt = +0.5,
    /* Sample time (in seconds) */
    .T = 0.005, // 采样时间 5ms
    /* Controller "memory" */
    .integrator = 0.0,
    .prevError = 0.0,
    .differentiator = 0.0,
    .prevMeasurement = 0.0,
    /* Controller output */
    .out = 0.0,
}; // PID 控制器实例

PIDController pid_s = {
    .kp = 0.15,
    .ki = 0.4,
    .kd = 0.0,
    /* 导数低通滤波器时间常数 */
    .tau = 0.0055,
    /* Output limits */
    .limMin = -1.0,
    .limMax = +1.0,
    /* Integrator limits */
    .limMinInt = -0.2,
    .limMaxInt = +0.2,
    /* Sample time (in seconds) */
    .T = 0.005, // 采样时间 5ms
    /* Controller "memory" */
    .integrator = 0.0,
    .prevError = 0.0,
    .differentiator = 0.0,
    .prevMeasurement = 0.0,
    /* Controller output */
    .out = 0.0,
}; // PID 控制器实例

PIDController pid_p = {
    .kp = 0.0,
    .ki = 0.0,
    .kd = 0.005,
    /* 导数低通滤波器时间常数 */
    .tau = 0.0055,
    /* Output limits */
    .limMin = -1.0,
    .limMax = +1.0,
    /* Integrator limits */
    .limMinInt = -0.5,
    .limMaxInt = +0.5,
    /* Sample time (in seconds) */
    .T = 0.005, // 采样时间 5ms
    /* Controller "memory" */
    .integrator = 0.0,
    .prevError = 0.0,
    .differentiator = 0.0,
    .prevMeasurement = 0.0,
    /* Controller output */
    .out = 0.0,
}; // PID 控制器实例

void speed_close_loop_init(void)
{
    float angle;
    MT6701_init();                  // 初始化 MT6701 传感器
    FOC_Calibrate_Angle();          // 校准角度
    MT6701_get_angle(NULL, &angle); // 获取初始角度
    FOC_AngleInit(angle);           // 初始化角度

    PIDController_Init(&pid_q); // 初始化 PID 控制器
    PIDController_Init(&pid_d); // 初始化 PID 控制器
    PIDController_Init(&pid_s); // 初始化 PID 控制器
}

float position_t = 0.0f; // 目标位置

float Speed_t = 0.0f;              // 目标速度
float Speed_r = 0.0f, Speed_l = 0; // 实际速度
float I_t_q = 0.0f, I_t_d = 0.0f;  // 目标电流

float angle_raw, angle_elec, angle_full;

float Ia, Ib, Ic, Ib_l = 0, Ic_l = 0;
uint16_t Ic_raw, Ib_raw;
float Ialpha, Ibeta, Id, Iq, Iq_last = 0, Id_last = 0;

float Uq, Ud = 0, Uq_l = 0, Ud_l = 0;
float Ualpha, Ubeta;
float v[3];


VOFA_ADD_DATA(Speed_t, VOFA_SAMPLE_FLOAT, &Speed_t);
VOFA_ADD_DATA(Speed_r, VOFA_SAMPLE_FLOAT, &Speed_r);
VOFA_ADD_DATA(kp, VOFA_SAMPLE_FLOAT, &pid_p.kp);
VOFA_ADD_DATA(ki, VOFA_SAMPLE_FLOAT, &pid_p.ki);
VOFA_ADD_DATA(Iq, VOFA_SAMPLE_FLOAT, &Iq);
VOFA_ADD_DATA(I_t_q, VOFA_SAMPLE_FLOAT, &I_t_q);
VOFA_ADD_DATA(speed_i, VOFA_SAMPLE_FLOAT, &pid_p.integrator);
VOFA_ADD_DATA(position_t, VOFA_SAMPLE_FLOAT, &position_t);
VOFA_ADD_DATA(angle_full, VOFA_SAMPLE_FLOAT, &angle_full);

void speed_close_loop_update(void)
{
    /* 角度 速度 位置 */
    MT6701_get_angle(NULL, &angle_raw);               // 获取原始角度
    FOC_FullAngle(&angle_raw, &angle_full, &Speed_r); // 获取全角度和转速
    FOC_elec_Angle(&angle_raw, &angle_elec);          // 获取电角度
    Speed_r = 0.2 * Speed_r + 0.8 * Speed_l;          // 简单低通滤波

    /* 电流采集 */
    CurrentSenser_get_current(2, &Ic_raw, &Ic); // 获取 C 相电流
    CurrentSenser_get_current(3, &Ib_raw, &Ib); // 获取 B 相电流
    Ib = 0.5 * (Ib + Ib_l);
    Ic = 0.5 * (Ic + Ic_l); // 简单低通滤波
    Ib_l = Ib;
    Ic_l = Ic;
    Ia = -(Ic + Ib); // C 相电流为 A 相和 B 相电流的负和

    /* 位置环 PID 控制 */
    Speed_t = PIDController_Update(&pid_p, position_t, angle_full); // 更新 PID 控制器

    /* 速度环 PID 控制 */
    I_t_q = PIDController_Update(&pid_s, Speed_t, Speed_r); // 更新 PID 控制器

    /* 电流变换 */
    FOC_clark_transform(TRANFORM_FORWARD, &Ia, &Ib, &Ic, &Ialpha, &Ibeta);        // Clark 变换
    FOC_park_transform(TRANFORM_FORWARD, &angle_elec, &Ialpha, &Ibeta, &Id, &Iq); // Park 变换
    Iq = 0.2 * Iq + 0.8 * Iq_last;
    Iq_last = Iq; // 低通滤波，平滑电流
    Id = 0.2 * Id + 0.8 * Id_last;
    Id_last = Id; // 低通滤波，平滑电流

    /* 电流环 PID 控制*/
    Uq = -PIDController_Update(&pid_q, I_t_q, Iq); // 更新 PID 控制器
    Ud = -PIDController_Update(&pid_d, I_t_d, Id); // 更新 PID 控制器
    Uq = 0.5 * (Uq + Uq_l);
    Uq_l = Uq;
    Ud = 0.5 * (Ud + Ud_l);
    Ud_l = Ud;

    FOC_park_transform(TRANFORM_INVERSE, &angle_elec, &Ualpha, &Ubeta, &Ud, &Uq); // 逆 Park 变换
    SVPWM(Ualpha, Ubeta, v);                                                      // 计算调制占空比
    MotorDriver_set_volatge_three(v[0], v[1], v[2]);                              // 输出电压
}
